from pb.ros.geometry_msgs import Pose_pb2
from pb.ros.geometry_msgs import PoseStamped_pb2
from pb.ros.nav_msgs import Odometry_pb2
from pb.ros.visualization_msgs import Marker_pb2
from pb.ros.geometry_msgs import Point_pb2
import proxy

import os
import time
import math
import threading
import sys

node = proxy.Connector('tcp://172.22.56.12:15555')  # 在709的电脑上使用该IP


# node = proxy.Connector('tcp://12.12.12.204:15555') #在集群的电脑上使用该IP
def callback_dp(pos):
    x1 = pos.position.x
    y1 = pos.position.y
    z1 = pos.position.z
    x = pos.orientation.x
    y = pos.orientation.y
    z = pos.orientation.z
    w = pos.orientation.w
    print("desired_pos:", pos, '\n')


def callback_odom(odom):
    x2 = odom.pose.pose.position.x
    y2 = odom.pose.pose.position.y
    z2 = odom.pose.pose.position.z
    x = odom.pose.pose.orientation.x
    y = odom.pose.pose.orientation.y
    z = odom.pose.pose.orientation.z
    w = odom.pose.pose.orientation.w
    print("odom position:", odom.pose.pose.position.x, '\n')


def callback_state(state):
    x3 = state.pose.pose.position.x
    y3 = state.pose.pose.position.y
    z3 = state.pose.pose.position.z
    x = state.pose.pose.orientation.x
    y = state.pose.pose.orientation.y
    z = state.pose.pose.orientation.z
    w = state.pose.pose.orientation.w
    # print("world position:", state, '\n')


def callback_marker(marker):
    marker_data = [dist for dist in marker.points]
    x = marker_data[0].x
    y = marker_data[0].y
    z = marker_data[0].z
    # print("expected trajectory:", marker_data, '\n')


subcribe_dp = node.Subscribe('FK', 'M300_0', '/FK/M300_0/desired_pos', Pose_pb2.Pose, callback_dp)  # 期望位置控制指令---相对于起点位置
subcribe_odom = node.Subscribe('FK', 'M300_0', '/FK/M300_0/odom', Odometry_pb2.Odometry, callback_odom)  # 里程计---相对于起点位置
subcribe_state = node.Subscribe('FK', 'M300_0', '/FK/M300_0/state', Odometry_pb2.Odometry, callback_state)  # 世界位置
subcribe_marker = node.Subscribe('FK', 'M300_0', '/FK/M300_0/marker', Marker_pb2.Marker,callback_marker)  # 期望轨迹（离散）---相对于起点位置


def sub_marker():
    while 1:
        subcribe_marker.SpinOnce()


def sub_state():
    while 1:
        subcribe_state.SpinOnce()


def sub_odom():
    while 1:
        subcribe_odom.SpinOnce()


def sub_dp():
    while 1:
        subcribe_dp.SpinOnce()


def main(args):
    thread_sub_dp = threading.Thread(target=sub_dp)
    thread_sub_dp.start()

    thread_sub_odom = threading.Thread(target=sub_odom)
    thread_sub_odom.start()

    thread_sub_state = threading.Thread(target=sub_state)
    thread_sub_state.start()

    thread_sub_marker = threading.Thread(target=sub_marker)
    thread_sub_marker.start()

    n = input('')


if __name__ == '__main__':
    main(sys.argv)
